#include "wifibot/wifibot.h"

#define DEF_LSM_COEFS 2
#define INTER_POINTS 10
#define START 10.0
#define STOP 150.0

namespace Lio {

// WifibotCalibration - Class For The Application:
// FlatApplication - base class, provides all necessary windows routines
class WifibotCalibration:public WifibotApplication
{
public:
protected:
	enum ChnlsLSMid{
		CHNL_DATA_IRL,
		CHNL_DATA_IRR,
		CHNL_DATA_DL,
		CHNL_DATA_DR,
		CHNL_LSM_IRL,
		CHNL_LSM_IRR,
		CHNL_LSM_DL,
		CHNL_LSM_DR,
		CHNL_LSM_COUNT
	};
	double meanSum,oldMeanSum,time;
	Channel *chnls[CHNL_LSM_COUNT];
	int coefCount,countL,countR;
	Vector<double>	dataDL,dataDR,dataIRL,dataIRR,lsmDataIRL,lsmDataIRR,
					coefsL,coefsR,
					lsmIRR,lsmIRL,lsmDR,lsmDL;
	// IR statistics:
	Stat statIRL,statIRR;
	// LSM datas:
	List<double> _dataDL,_dataDR;
	List<Stat> _dataIRL,_dataIRR;
	BOOL resultl,resultr;

	// Window controls: Labels, TextBoxes, Scrolls and Buttons for the Main Window
	FlatGraph fgLSM;
	FlatLabel lDist;
	FlatText tbDist,tbLSM,tbCoefL,tbCoefR;
	FlatButton bAdd,bClear,bExport,bImport,bCalibrate,bLSM,bPlus,bMinus;
	FlatSelect sSampleL,sSampleR;

	void dump(FILE *file);
	void draw2D();
	void addDist(double dist);
	void showData();
	BOOL setDataSize(int sizeL, int sizeR);
	BOOL buildData();
	void readData();
	BOOL calcLSM();
	void buildLSMGraph();
	void showCoefs();
	BOOL setLSMCoefs(int coefs);
	void exportCallibration();
	void importCallibration();
	//void updateAction();
	BOOL listenAction(WORD id, WORD code, const void *info);
	void connectAction();
	BOOL createAction();
	void reshapeAction();
	void receiveAction();
	void recalc();
} app;
void WifibotCalibration::connectAction()
{
	time=0;
	if(bConnect.checked)
		fgIR.graph.clear(TRUE);
}
void WifibotCalibration::reshapeAction()
{
	fgLSM.size.copy(fgIR.size);
	tbCoefL.size.x=tbCoefR.size.x=sSampleL.size.x=sSampleR.size.x=float(0.5*window.width-15);
}
void WifibotCalibration::receiveAction()
{
	if(bCalibrate.checked)
	{
		statIRL.add(robotSensors.IRLeft);
		statIRR.add(robotSensors.IRRight);
		statIRL.update();
		statIRR.update();
		oldMeanSum=meanSum;
		meanSum=statIRL.mean+statIRR.mean;
	}
}
void WifibotCalibration::recalc()
{
	showData();
	calcLSM();
}
void WifibotCalibration::addDist(double dist)
{
	_dataDL.add(dist);
	_dataDR.add(dist);
	_dataIRL.add(statIRL);
	_dataIRR.add(statIRR);
	buildData();
	recalc();
}
void WifibotCalibration::showData()
{
	int i;
	const int size=100;
	_TCHAR str[size];
	sSampleL.clear();
	for(i=0;i<dataDL.size;i++)
	{
		_sntprintf(str,size,_T("%2d. ir=%6.3f dist=%6.3f"),i+1,dataIRL.elems[i],dataDL.elems[i]);
		sSampleL.add(str);
	}
	sSampleL.build();
	sSampleR.clear();
	for(i=0;i<dataDR.size;i++)
	{
		_sntprintf(str,size,_T("%2d. ir=%6.3f dist=%6.3f"),i+1,dataIRR.elems[i],dataDR.elems[i]);
		sSampleR.add(str);
	}
	sSampleR.build();
}
BOOL WifibotCalibration::setDataSize(int sizeL, int sizeR)
{
	//if(sizeL<2||sizeR<2)
	//	return FALSE;

	dataDL.setSize(sizeL);
	dataDR.setSize(sizeR);
	dataIRL.setSize(sizeL);
	dataIRR.setSize(sizeR);
	lsmDataIRL.setSize(sizeL);
	lsmDataIRR.setSize(sizeR);

	chnls[CHNL_DATA_IRL]->setSource(dataIRL);
	chnls[CHNL_DATA_IRR]->setSource(dataIRR);
	chnls[CHNL_DATA_DL]->setSource(dataDL);
	chnls[CHNL_DATA_DR]->setSource(dataDR);

	countL=(sizeL-1)*INTER_POINTS;
	countR=(sizeR-1)*INTER_POINTS;
	lsmIRR.setSize(countR);
	lsmIRL.setSize(countL);
	lsmDR.setSize(countR);
	lsmDL.setSize(countL);
	chnls[CHNL_LSM_IRL]->setSource(lsmIRL);
	chnls[CHNL_LSM_IRR]->setSource(lsmIRR);
	chnls[CHNL_LSM_DL]->setSource(lsmDL);
	chnls[CHNL_LSM_DR]->setSource(lsmDR);

	return TRUE;
}
BOOL WifibotCalibration::buildData()
{
	if(!setDataSize(_dataDL.length,_dataDR.length))
		return FALSE;
	sSampleL.clear();
	sSampleR.clear();
	int i;
	ListObject<double> *objDistL=_dataDL.first,*objDistR=_dataDR.first;
	ListObject<Stat> *objIRL=_dataIRL.first,*objIRR=_dataIRR.first;
	for(i=0;objDistL;i++)
	{
		dataDL[i]=objDistL->value;
		dataIRL[i]=objIRL->value.mean;
		objDistL=objDistL->next;
		objIRL=objIRL->next;
	}
	for(i=0;objDistR;i++)
	{
		dataDR[i]=objDistR->value;
		dataIRR[i]=objIRR->value.mean;
		objDistR=objDistR->next;
		objIRR=objIRR->next;
	}
	return TRUE;
}
void WifibotCalibration::readData()
{
	_dataDL.destroy();
	_dataDR.destroy();
	_dataIRL.destroy();
	_dataIRR.destroy();
	int i;
	for(i=0;i<dataDL.size;i++)
	{
		_dataDL.add(dataDL.elems[i]);
		_dataIRL.add()->mean=dataIRL.elems[i];
	}
	for(i=0;i<dataDR.size;i++)
	{
		_dataDR.add(dataDR.elems[i]);
		_dataIRR.add()->mean=dataIRR.elems[i];
	}
}
void WifibotCalibration::exportCallibration()
{
	//buildData();
	File file;
	if(file.open(_T("IRR.dat"),_T("wb")))
	{
		file.write(&dataDR.size,1);
		file.write(dataIRR.elems,dataDR.size);
		file.write(dataDR.elems,dataDR.size);
		file.close();
	}
	if(file.open(_T("IRL.dat"),_T("wb")))
	{
		file.write(&dataDL.size,1);
		file.write(dataIRL.elems,dataDL.size);
		file.write(dataDL.elems,dataDL.size);
		file.close();
	}
}
void WifibotCalibration::importCallibration()
{
	int sizeL,sizeR;
	File fileL,fileR;
	if(	fileR.open(_T("IRR.dat"),_T("rb"))&&
		fileL.open(_T("IRL.dat"),_T("rb")))
	{
		fileR.read(&sizeR,1);
		fileL.read(&sizeL,1);
		if(setDataSize(sizeL,sizeR))
		{
			fileR.read(dataIRR.elems,sizeR);
			fileR.read(dataDR.elems,sizeR);
			fileL.read(dataIRL.elems,sizeL);
			fileL.read(dataDL.elems,sizeL);
			readData();
			//buildData();
			recalc();
		}
		fileL.close();
		fileR.close();
	}
}
BOOL WifibotCalibration::setLSMCoefs(int coefs)
{
	if(coefs<2)
		coefs=2;
	coefCount=coefs;
	tbLSM.setText(format.toString(coefs));
	coefsL.setSize(coefs);
	coefsR.setSize(coefs);
	calcLSM();
	return TRUE;
}
void WifibotCalibration::showCoefs()
{
	int i;
	format.precision=6;
	tbCoefL.text.copy(_T("coefL={"));
	tbCoefL.text.add(format.toString(coefsL.elems[0]));
	for(i=1;i<coefsL.size;i++)
	{
		tbCoefL.text.add(',');
		tbCoefL.text.add(format.toString(coefsL.elems[i]));
	}
	tbCoefL.text.add('}');
	tbCoefL.applyTextChages();
	tbCoefR.text.copy(_T("coefR={"));
	tbCoefR.text.add(format.toString(coefsR.elems[0]));
	for(i=1;i<coefsR.size;i++)
	{
		tbCoefR.text.add(',');
		tbCoefR.text.add(format.toString(coefsR.elems[i]));
	}
	tbCoefR.text.add('}');
	tbCoefR.applyTextChages();
	format.precision=3;
}
BOOL WifibotCalibration::calcLSM()
{
	int i;
	for(i=0;i<dataDL.size;i++)
		lsmDataIRL.elems[i]=INV(dataIRL.elems[i]);
	for(i=0;i<dataDR.size;i++)
		lsmDataIRR.elems[i]=INV(dataIRR.elems[i]);
	if(	!dataDL.size||!dataDR.size||
		!getLSMPolyCoef(coefsL.elems,coefsL.size,dataDL.elems,lsmDataIRL.elems,dataDL.size)||
		!getLSMPolyCoef(coefsR.elems,coefsR.size,dataDR.elems,lsmDataIRR.elems,dataDR.size))
		return FALSE;
	showCoefs();
	buildLSMGraph();
	return TRUE;
}
void WifibotCalibration::buildLSMGraph()
{
	int i;
	double step,arg;

	step=(STOP-START)/(countL-1);
	for(i=0,arg=START;i<countL;i++,arg+=step)
	{
		lsmIRL.elems[i]=arg;
		lsmDL.elems[i]=coefsL.elems[0]+coefsL.elems[1]/arg;
	}
	step=(STOP-START)/(countR-1);
	for(i=0,arg=START;i<countR;i++,arg+=step)
	{
		lsmIRR.elems[i]=arg;
		lsmDR.elems[i]=coefsR.elems[0]+coefsR.elems[1]/arg;
	}
	fgLSM.graph.update();
}
BOOL WifibotCalibration::createAction()		
{
	name=_T("Wifibot Callibration");

	oldMeanSum=meanSum=0;

	bLSM.text=_T("LSM");
	bLSM.copy(bMem);
	bLSM.stick(DIR_DOWN,0,10,&bFilter);

	tbLSM.text=_T("5");
	tbLSM.copy(tbMem);
	tbLSM.size.x=30;
	tbLSM.stick(DIR_RIGHT,5,0);

	bPlus.text=_T("+");
	bPlus.copy(bMem);
	bPlus.stick(DIR_RIGHT,5,0);

	bMinus.text=_T("-");
	bMinus.copy(bPlus);
	bMinus.stick(DIR_RIGHT,5,0);

	bCalibrate.text=_T("Cal");
	bCalibrate.copy(bConnect);
	bCalibrate.pressColor=INT_WRED;
	bCalibrate.stick(DIR_RIGHT,5,0);

	lDist.text=_T("Dist:");
	lDist.fontColor=INT_BLACK;
	lDist.margin.set(5,1);
	lDist.adjustSize=TRUE;
	lDist.stick(DIR_RIGHT,5,0);

	tbDist.copy(tbMem);
	tbDist.text="0.000";
	tbDist.size.x=60;
	tbDist.stick(DIR_RIGHT,5,0);

	bAdd.text=_T("Add");
	bAdd.copy(bMem);
	bAdd.stick(DIR_RIGHT,5,0);

	bClear.text=_T("Clear");
	bClear.copy(bAdd);
	bClear.stick(DIR_RIGHT,5,0);

	bExport.text=_T("Export");
	bExport.copy(bMem);
	bExport.stick(DIR_RIGHT,5,0);

	bImport.text=_T("Import");
	bImport.copy(bMem);
	bImport.stick(DIR_RIGHT,5,0);

	sSampleL.size.x=200;
	sSampleL.margin.set(5,5);
	sSampleL.readLines=8;
	sSampleL.fontColor=INT_DGRAY;
	sSampleL.bgColor=INT_WHITE;
	sSampleL.borderColor=INT_BLACK;
	sSampleL.stick(DIR_DOWN,0,20,&bLSM);

	sSampleR.copy(sSampleL);
	sSampleR.stick(DIR_RIGHT,10,0);

	tbCoefL.copy(tbMem);
	tbCoefL.stick(DIR_DOWN,0,5,&sSampleL);

	tbCoefR.copy(tbMem);
	tbCoefR.stick(DIR_DOWN,0,5,&sSampleR);

	fgLSM.copy(fgIR);
	fgLSM.stick(DIR_RIGHT,10,0,&fgIR);
	fgLSM.chnlReadLines=2;
	fgLSM.graph.limY.set(0,1.7);

	chnls[CHNL_LSM_IRL]=fgLSM.graph.addChannel(_T("IRL"));
	chnls[CHNL_LSM_IRR]=fgLSM.graph.addChannel(_T("IRR"));
	chnls[CHNL_LSM_DL]=fgLSM.graph.addChannel(_T("DistL=f(IRL)"),_T("IRL"),INT_BLUE);
	chnls[CHNL_LSM_DR]=fgLSM.graph.addChannel(_T("DistR=f(IRR)"),_T("IRR"),INT_RED);

	chnls[CHNL_DATA_IRL]=fgLSM.graph.addChannel(_T("_IRL"));
	chnls[CHNL_DATA_IRR]=fgLSM.graph.addChannel(_T("_IRR"));
	chnls[CHNL_DATA_DL]=fgLSM.graph.addChannel(_T("Samples DistL"),_T("_IRL"),INT_BLUE,0,NULL,-10);
	chnls[CHNL_DATA_DR]=fgLSM.graph.addChannel(_T("Samples DistR"),_T("_IRR"),INT_RED,0,NULL,-10);
	
	// Include the controls in the window

	add(&bLSM);
	add(&tbLSM);
	add(&bPlus);
	add(&bMinus);
	add(&bCalibrate);
	add(&lDist);
	add(&tbDist);
	add(&bAdd);
	add(&bClear);
	add(&bExport);
	add(&bImport);
	add(&sSampleL);
	add(&sSampleR);
	add(&tbCoefL);
	add(&tbCoefR);
	add(&fgLSM);

	setLSMCoefs(DEF_LSM_COEFS);
	
	return TRUE;
}
BOOL WifibotCalibration::listenAction(WORD id, WORD code, const void *info)
{
	if(id==bCalibrate.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			if(bCalibrate.checked)
			{
				statIRL.clear();
				statIRR.clear();
			}
			return TRUE;
		}
	}
	else if(id==bExport.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			exportCallibration();
		}
	}
	else if(id==bImport.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			importCallibration();
		}
	}
	else if(id==bLSM.id||id==tbLSM.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			setLSMCoefs(format.toInt(tbLSM.getText(),DEF_LSM_COEFS));
		}
	}
	else if(id==bPlus.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			setLSMCoefs(coefCount+1);
		}
	}
	else if(id==bMinus.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			setLSMCoefs(coefCount-1);
		}
	}
	else if(id==bAdd.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			bCalibrate.checked=FALSE;
			double dist=format.toDouble(tbDist.getText(),0);
			addDist(dist);
			return TRUE;
		}
	}
	else if(id==bClear.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			_dataDL.destroy();
			_dataDR.destroy();
			dataIRL.destroy();
			dataIRR.destroy();
			dataDL.destroy();
			dataDR.destroy();
			dataIRL.destroy();
			dataIRR.destroy();
			lsmDataIRL.destroy();
			lsmDataIRR.destroy();
			sSampleL.clear();
			sSampleR.clear();
			return TRUE;
		}
	}
	else if(id==sSampleL.id)
	{
		if(code==FLAT_ACTION_DELETE)
		{
			int index=sSampleL.getSelectedIndex();
			_dataDL.remove(index);
			_dataIRL.remove(index);
			buildData();
			recalc();
			return TRUE;
		}
	}
	else if(id==sSampleR.id)
	{
		if(code==FLAT_ACTION_DELETE)
		{
			int index=sSampleR.getSelectedIndex();
			_dataDR.remove(index);
			_dataIRR.remove(index);
			buildData();
			recalc();
			return TRUE;
		}
	}
	return FALSE;
}
void WifibotCalibration::draw2D()
{
	font.color=INT_BLACK;
	font.x=10.0f;
	font.y=bLSM.getRect().bottom()+15.0f;
	glPrint(&font,_T("IRL: m=%5.3f d=%5.3f s=%+6.3f e=%+6.3f"),statIRL.mean,statIRL.stdev,statIRL.skew,statIRL.excess);
	font.x=0.5f*window.width+5.0f;
	glPrint(&font,_T("IRR: m=%5.3f d=%5.3f s=%+6.3f e=%+6.3f"),statIRR.mean,statIRR.stdev,statIRR.skew,statIRR.excess);
	
	fontBold.color=INT_DGREEN;
	fontBold.x=660.0f;
	fontBold.y=100.0f;
	glPrint(&fontBold,_T("DIV:"));
	fontBold.color=INT_BLACK;
	fontBold.x+=50.0f;
	glPrint(&fontBold,_T("%f"),absval(oldMeanSum-meanSum));
}

} // namespace Lio

int WINAPI _tWinMain(HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nCmdShow)
{
	return Lio::app.main(hInstance,hPrevInstance,lpCmdLine,nCmdShow);
}